import time


def _constrain(value, low, high):
    if value > high:
        return high
    elif value < low:
        return low
    else:
        return value


class PIDController(object):

    def __init__(self, P, I, D, ramp, limit) -> None:
        self.P = float(P)
        self.I = float(I)
        self.D = float(D)
        self.output_ramp = ramp
        self.limit = limit
        self.integral_prev = 0.0
        self.error_prev = 0.0
        self.output_prev = 0.0
        self.time_prev = time.time()

    def calc(self, error):
        # calculate the time from the last call
        time_now = time.time()
        Ts = (time_now - self.time_prev)
        # quick fix for strange cases (micros overflow)
        if (Ts <= 0 or Ts > 0.5):
            Ts = 1e-3

        # u(s) = (P + I/s + Ds)e(s)
        # Discrete implementations
        # proportional part
        # u_p  = P *e(k)
        proportional = self.P * error
        # Tustin transform of the integral part
        # u_ik = u_ik_1  + I*Ts/2*(ek + ek_1)
        integral = self.integral_prev + self.I * Ts * 0.5 * (error + self.error_prev)
        # antiwindup - limit the output
        integral = _constrain(integral, -self.limit, self.limit)
        # Discrete derivation
        # u_dk = D(ek - ek_1)/Ts
        derivative = self.D * (error - self.error_prev) / Ts

        # sum all the components
        output = proportional + integral + derivative
        # antiwindup - limit the output variable
        output = _constrain(output, -self.limit, self.limit)

        # if output ramp defined
        if (self.output_ramp > 0):
            # limit the acceleration by ramping the output
            output_rate = (output - self.output_prev) / Ts
            if (output_rate > self.output_ramp):
                output = self.output_prev + self.output_ramp * Ts
            elif (output_rate < -self.output_ramp):
                output = self.output_prev - self.output_ramp * Ts

        # saving for the next pass
        self.integral_prev = integral
        self.output_prev = output
        self.error_prev = error
        self.time_prev = time_now
        return output


'''
PIDController::PIDController(float P, float I, float D, float ramp, float limit)
    : P(P)
    , I(I)
    , D(D)
    , output_ramp(ramp)    # output derivative limit [volts/second]
    , limit(limit)         # output supply limit     [volts]
    , integral_prev(0.0)
    , error_prev(0.0)
    , output_prev(0.0)
{
    timestamp_prev = _micros();
}

# PID controller function
float PIDController::operator() (float error){
    # calculate the time from the last call
    unsigned long timestamp_now = _micros();
    float Ts = (timestamp_now - timestamp_prev) * 1e-6;
    # quick fix for strange cases (micros overflow)
    if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; 

    # u(s) = (P + I/s + Ds)e(s)
    # Discrete implementations
    # proportional part 
    # u_p  = P *e(k)
    float proportional = P * error;
    # Tustin transform of the integral part
    # u_ik = u_ik_1  + I*Ts/2*(ek + ek_1)
    float integral = integral_prev + I*Ts*0.5*(error + error_prev);
    # antiwindup - limit the output
    integral = _constrain(integral, -limit, limit);
    # Discrete derivation
    # u_dk = D(ek - ek_1)/Ts
    float derivative = D*(error - error_prev)/Ts;

    # sum all the components
    float output = proportional + integral + derivative;
    # antiwindup - limit the output variable
    output = _constrain(output, -limit, limit);

    # if output ramp defined
    if(output_ramp > 0){
        # limit the acceleration by ramping the output
        float output_rate = (output - output_prev)/Ts;
        if (output_rate > output_ramp)
            output = output_prev + output_ramp*Ts;
        else if (output_rate < -output_ramp)
            output = output_prev - output_ramp*Ts;
    }
    # saving for the next pass
    integral_prev = integral;
    output_prev = output;
    error_prev = error;
    timestamp_prev = timestamp_now;
    return output;
}
'''